package clubhouse.visualisation;

import java.awt.BasicStroke;
import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Component;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Label;
import java.awt.TextArea;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.awt.geom.RoundRectangle2D;
import java.awt.print.PageFormat;
import java.awt.print.Printable;
import java.awt.print.PrinterException;
import java.awt.print.PrinterJob;
import java.io.FileNotFoundException;
import java.util.Vector;

import javax.swing.BoxLayout;
import javax.swing.JApplet;
import javax.swing.JButton;
import javax.swing.JCheckBox;
import javax.swing.JComboBox;
import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JOptionPane;
import javax.swing.JPanel;
import javax.swing.JTextField;
import javax.swing.RepaintManager;
import javax.swing.SwingUtilities;
import clubhouse.base.Base;
import clubhouse.control.DataChecker;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class WSNexperimentation extends JApplet implements Runnable, ActionListener, MouseListener {
	
	/**Universal version IDentifier for Serializable class.*/
	private static final long serialVersionUID = 1L;
	
	/**The checkBox used to do or not the 4 stage complete simulation.*/
	private JCheckBox viewSimulation = new JCheckBox("Simulate");
	/**The checkBox used to do or not the Anti-SLAM comparison simulation.*/
	private JCheckBox viewCompareAntiSlam = new JCheckBox("Compare Anti-SLAM");
	/**The checkBox used to display or not known landmarks.*/
	private JCheckBox viewLandmarks = new JCheckBox("Landmarks");
	/**The checkBox used to display or not the current robot position.*/
	private JCheckBox viewOdometry = new JCheckBox("Robot position");
	/**The checkBox used to display or not all previous and current robot position.*/
	private JCheckBox viewTrajectory = new JCheckBox("Trajectory");
	/**The checkBox used to display or not all previous and current robot position in an initial trajectory.*/
	private JCheckBox viewReal = new JCheckBox("Initial Trajectory");
	/**The checkBox used to display or not all previous and current robot position in an error trajectory.*/
	private JCheckBox viewError = new JCheckBox("Error Trajectory");
	/**The checkBox used to display or not all previous and current robot position in an EKF trajectory.*/
	private JCheckBox viewFilter = new JCheckBox("EKF Trajectory");
	/**The checkBox used to display or not all previous and current robot position in an Anti-SLAM trajectory.*/
	private JCheckBox viewAnti = new JCheckBox("A-S Trajectory");
	/**The checkBox used to display or not Voronoi polygons.*/
	private JCheckBox viewVoronoi = new JCheckBox("Show Voronoi");
	/**The checkBox used to display or not the communication and acting range of each known landmarks.*/
	private JCheckBox viewRange = new JCheckBox("Show Landmarks' Range");
	/**The checkBox used to enable or not the Anti-SLAM process in a simple simulation.*/
	private JCheckBox enableAntiSlam = new JCheckBox("Anti-Slam Process");
	/**The checkBox used to enable or not the transmission error process in a simple simulation.*/
	private JCheckBox enableError = new JCheckBox("Transmission Error");
	/**The checkBox used to enable or not the EKF process in a simple simulation.*/
	private JCheckBox enableEkf = new JCheckBox("EKF");
	/**Comparing Anti Slam**/
	/**The checkBox used to display or not all previous and current robot position in an Translation Anti-SLAM 
	 * Strategy trajectory.*/
	private JCheckBox viewASTranslation = new JCheckBox("Translation Trajectory");
	/**The checkBox used to display or not all previous and current robot position in an Rotation Anti-SLAM 
	 * Strategy trajectory.*/
	private JCheckBox viewASRotation = new JCheckBox("Rotation Trajectory");
	/**The checkBox used to display or not all previous and current robot position in an Hybrid Anti-SLAM 
	 * Strategy trajectory.*/
	private JCheckBox viewASHybrid = new JCheckBox("Hybrid Trajectory");
	/**The Panel containing the label and the field allowing to choose the Anti-SLAM strategy.*/
	JPanel ASChoicePanel = new JPanel();
	/**The field used to choose the Anti-SLAM strategy if Anti-SLAM is enabled.*/
	private JComboBox ASChoice = new JComboBox(new String[]{"Translation","Rotation","Hybrid"});
	/***********************/
	/**The field used to choose the scenario.*/
	private JComboBox scenarioChoice = new JComboBox(new String[]{"1","2","3"});
	/**The field used to choose the error attenuation factor.*/
	private JTextField errorFactor = new JTextField("10", 3);
	/**The field used to choose the way to generate network, in case of scenario 3*/
	private JComboBox landmarkChoice = new JComboBox(new String[]{"Random","From file","From panel","Real"});
	/**The field used to choose the number of node to generate, in case of scenario 3*/
	private JTextField numberOfLandmarks = new JTextField("10", 3);
	/**The field used to choose the number of simulations to run*/
	private JTextField numberOfSimulations = new JTextField("1", 2);
	/**The field used to choose the name of the file to read a network or to save it.*/
	private JTextField fileName = new JTextField("scenario", 10);
	/**The button to generate the network by reading the file specified in the fileName field.*/
	private JButton openFile = new JButton("Open");
	/**The button to save the current network in the file specified in the fileName field.*/
	private JButton saveFile = new JButton("Save");
	/**The button to save the current drawing by opening a Print window.*/
	private JButton saveGraph = new JButton("Save Graph");
	/**The button to save the current metrics in specific file.*/
	private JButton saveInfo = new JButton("Save Metrics");

	/**The button to launch the simulation.*/
	private JButton startButton = new JButton("Start");
	/**The button to pause the current simulation.*/
	private JButton stopButton = new JButton("Stop");
	/**The button to resume the current simulation after being paused.*/
	private JButton resumeButton = new JButton("Resume");
	/**The button to clear the current simulation.*/
	private JButton clearButton = new JButton("Clear");

	/**The panel which display the log related to the Xbee communication.*/
	private TextArea messageLog = new TextArea();
	/**The panel which display the log related to the simulation itself.*/
	private TextArea debugLog = new TextArea();
	/**The panel which display the metric M1 after the simulation is paused or cleared.*/
	private TextArea M1Display = new TextArea();
	/**The panel which display the metric M2 after the simulation is paused or cleared.*/
	private TextArea M2Display = new TextArea();
	/**The panel which display the metric M3 after the simulation is paused or cleared.*/
	private TextArea M3Display = new TextArea();
	/**The panel which display the metric M4 after the simulation is paused or cleared.*/
	private TextArea M4Display = new TextArea();
	
	/**The Base Station instance.*/
	public Base test = new Base(this);
	/**The Base Station independent thread.*/
	public Thread t;
	/**The DataChecker object used to check and format data before displaying in logs.*/
	private DataChecker checker;
	/**Current scenario.*/
	private int scenario = 1;
	
	/**The current set of user nodes added on the panel.*/
	private Vector<double[]> userLandmarks = new Vector<double[]>();
	/**The current user file name.*/
	private String userFileName = "";
	/**The current number of user node added on the panel by the user.*/
	private int userId = 1;
	/**Boolean to allow or not the user to add node on the panel.*/
	private boolean getPanelNodes = false;
	/**Boolean to know if current user network has been saved or not.*/
	private boolean saved;
	
	/**DrawPanel object*/
	private DrawPanel experimentation = new DrawPanel(this,this.scenario);


	
	/** See following "See Also".
	 * @see java.applet.Applet#init()
	 */
	public void init () {
        try {
        	SwingUtilities.invokeAndWait(this);
        } catch (Exception e) {
        	System.err.println("Initialization failure");
        }
    }
	
	/**
	 * Generate interface and add actionListener.
	 * <br/> See following "See Also".
	 * @see java.lang.Runnable#run()
	 */
	public void run() {
		this.t = new Thread(test);
		this.checker = new DataChecker();
		
		setLayout(new BorderLayout());
		
    	JPanel scenarioPanel = new JPanel();
        scenarioPanel.add(new JLabel("Scenario "), "West");
        scenarioPanel.add(scenarioChoice, "East");
        
        JPanel landmarksPanel = new JPanel();
        landmarksPanel.add(new JLabel("Landmarks "));
        landmarksPanel.add(landmarkChoice);
        landmarksPanel.add(new JLabel("Number of Landmarks "));
        landmarksPanel.add(numberOfLandmarks);
        numberOfLandmarks.setEditable(false);
        fileName.setEnabled(false);
        
        JPanel filePanel = new JPanel();
        filePanel.add(fileName, "West");
        filePanel.add(openFile, "Center");
        filePanel.add(saveFile, "East");
        openFile.setEnabled(false);
        saveFile.setEnabled(false);
        
        JPanel errorPanel = new JPanel();
        errorPanel.add(new JLabel("Error attenuation factor "), "West");
        errorPanel.add(errorFactor, "East");
        
        JPanel numberSimulationPanel= new JPanel();
        numberSimulationPanel.add(new JLabel("Sim: "), "West");
        numberSimulationPanel.add(numberOfSimulations,"East");
        
		JPanel paramatersPanel = new JPanel();
		paramatersPanel.setLayout(new BoxLayout(paramatersPanel, BoxLayout.X_AXIS));
		paramatersPanel.add(scenarioPanel);
		paramatersPanel.add(landmarksPanel);
		paramatersPanel.add(filePanel);
		paramatersPanel.add(errorPanel);
		paramatersPanel.add(numberSimulationPanel);
        
		JPanel buttonPanel = new JPanel();
		buttonPanel.add(startButton);
        buttonPanel.add(stopButton);
        buttonPanel.add(resumeButton);
        buttonPanel.add(clearButton);
        buttonPanel.add(paramatersPanel);
        stopButton.setEnabled(false);
        resumeButton.setEnabled(false);
        clearButton.setEnabled(false);
        this.add(buttonPanel, "North");
        
        JPanel switchPanel = new JPanel();
        switchPanel.setLayout(new BoxLayout(switchPanel, BoxLayout.Y_AXIS));
        switchPanel.add(viewSimulation);
        switchPanel.add(viewCompareAntiSlam);
        switchPanel.add(new JLabel("  "));
        switchPanel.add(viewLandmarks);
        switchPanel.add(viewOdometry);
        switchPanel.add(viewTrajectory);
        switchPanel.add(viewRange);
        switchPanel.add(viewVoronoi);
        switchPanel.add(viewReal);
        switchPanel.add(viewError);
        switchPanel.add(viewFilter);
        switchPanel.add(viewAnti);
        switchPanel.add(viewASTranslation);
        switchPanel.add(viewASRotation);
        switchPanel.add(viewASHybrid);
        switchPanel.add(new JLabel("  "));
        switchPanel.add(enableError);
        switchPanel.add(enableEkf);
        switchPanel.add(enableAntiSlam);
    	ASChoicePanel.add(new JLabel("AS Strategy"), "West");
    	ASChoicePanel.add(ASChoice, "Center");
        ASChoicePanel.setMaximumSize(new Dimension(200,30));
        ASChoicePanel.setAlignmentX(0);
        ASChoicePanel.setVisible(false);
        switchPanel.add(ASChoicePanel);
        switchPanel.add(new JLabel("  "));
        switchPanel.add(saveGraph);
        switchPanel.add(new JLabel("  "));
        switchPanel.add(saveInfo);
        saveGraph.setEnabled(false);
        saveInfo.setEnabled(false);
        viewVoronoi.setVisible(false);
        viewRange.setVisible(false);
		viewReal.setVisible(false);
		viewError.setVisible(false);
		viewFilter.setVisible(false);
		viewAnti.setVisible(false);
		viewASTranslation.setVisible(false);
		viewASRotation.setVisible(false);
		viewASHybrid.setVisible(false);
        viewLandmarks.setSelected(true);
        viewOdometry.setSelected(true);
        viewSimulation.setSelected(false);
        viewCompareAntiSlam.setSelected(false);
        viewTrajectory.setSelected(true);
        enableEkf.setSelected(false);
        this.add(switchPanel, "West");

        JPanel macroPanel = new JPanel();
        macroPanel.setLayout(new BorderLayout());
        
        //Debug Log Display
        JPanel tempPanel1 = new JPanel();
        tempPanel1.setLayout(new BorderLayout());
        JPanel debugPanel = new JPanel();
        debugPanel.setLayout(new BorderLayout());
        debugPanel.add(new Label("Debug Log:"), "North");
        debugLog.setBackground(Color.white);
        debugLog.setEditable(false);
        debugLog.setPreferredSize(new Dimension(600,250));
                
        debugPanel.add(debugLog);
        tempPanel1.add(debugPanel, "West");
        tempPanel1.add(new JLabel("   "), "Center");
        
        //M1 Display       
        //JPanel tempPanel1 = new JPanel();
        JPanel M1Panel = new JPanel();
        M1Panel.setLayout(new BorderLayout());
        M1Panel.add(new Label("M1:"), "North");
        M1Panel.setPreferredSize(new Dimension(150,250));
        this.M1Display.setBackground(Color.white);
        this.M1Display.setEditable(false);
        M1Panel.add(M1Display);
        tempPanel1.add(M1Panel, "East"); 
        //macroPanel.add(M1Panel,"Center");   
        
        //M2 Display       
        JPanel tempPanel2 = new JPanel();
        tempPanel2.setLayout(new BorderLayout());
        JPanel M2Panel = new JPanel();
        M2Panel.setLayout(new BorderLayout());
        M2Panel.add(new Label("M2:"), "North");
        this.M2Display.setBackground(Color.white);
        this.M2Display.setEditable(false);
        this.M2Display.setPreferredSize(new Dimension(150,250));
        M2Panel.add(M2Display);
        tempPanel2.add(new JLabel("   "), "West"); 
        tempPanel2.add(M2Panel, "Center");  
        tempPanel2.add(new JLabel("   "), "East"); 
        //macroPanel.add(M2Panel,"East");  
        
        //M3 Display       
        JPanel tempPanel3 = new JPanel();
        tempPanel3.setLayout(new BorderLayout());
        JPanel M3Panel = new JPanel();
        M3Panel.setLayout(new BorderLayout());
        M3Panel.add(new Label("M3:"), "North");
        this.M3Display.setBackground(Color.white);
        this.M3Display.setEditable(false);
        this.M3Display.setPreferredSize(new Dimension(150,250));
        M3Panel.add(M3Display, "Center");
        tempPanel3.add(M3Panel,"West");  
        tempPanel3.add(new JLabel("   "), "Center"); 
        
        //M4 Display       
        JPanel M4Panel = new JPanel();
        M4Panel.setLayout(new BorderLayout());
        M4Panel.add(new Label("M4:"), "North");
        this.M4Display.setBackground(Color.white);
        this.M4Display.setEditable(false);
        this.M4Display.setPreferredSize(new Dimension(150,250));
        M4Panel.add(M4Display, "Center");
        tempPanel3.add(M4Panel,"East");  
        
        macroPanel.add(tempPanel1, "West");
        macroPanel.add(tempPanel2, "Center");
        macroPanel.add(tempPanel3, "East");
        this.add(macroPanel, "South");
        
        JPanel messagePanel = new JPanel();
        messagePanel.setLayout(new BorderLayout());
        messagePanel.add(new Label("Message Log:"), "North");
        messageLog.setEnabled(true);
        messageLog.setEditable(false);
        messageLog.setBackground(Color.white);
        messagePanel.add(messageLog, "Center");
        this.add(messagePanel, "East");
        
        experimentation.setBackground(Color.lightGray);
        this.add(experimentation, "Center");
        
        startButton.addActionListener(this);
        stopButton.addActionListener(this);
        clearButton.addActionListener(this);
        resumeButton.addActionListener(this);
        viewLandmarks.addActionListener(this);
        viewOdometry.addActionListener(this);
        viewSimulation.addActionListener(this);
        viewCompareAntiSlam.addActionListener(this);
        viewTrajectory.addActionListener(this);
        enableAntiSlam.addActionListener(this);
        enableError.addActionListener(this);
        enableEkf.addActionListener(this);
        viewAnti.addActionListener(this);
        viewReal.addActionListener(this);
		viewError.addActionListener(this);
		viewFilter.addActionListener(this);
        viewASTranslation.addActionListener(this);
        viewASRotation.addActionListener(this);
        viewASHybrid.addActionListener(this);
        scenarioChoice.addActionListener(this);
        ASChoice.addActionListener(this);
        viewVoronoi.addActionListener(this);
        viewRange.addActionListener(this);
        
        landmarkChoice.addActionListener(this);
        landmarkChoice.setEnabled(false);
        openFile.addActionListener(this);
        saveFile.addActionListener(this);
        saveGraph.addActionListener(this);
        saveInfo.addActionListener(this);
        
        experimentation.addMouseListener(this);
	}

	/** See following "See Also".
	 * @see java.awt.event.ActionListener#actionPerformed(java.awt.event.ActionEvent)
	 */
	public void actionPerformed(ActionEvent e) {
		if (e.getSource() == clearButton) {
			//Clear stuff
			this.userId = 1;
			this.userLandmarks = new Vector<double[]>();
			experimentation.clear();
			test.clearRoutine();
	        this.messageLog.setText("");
	        this.debugLog.setText("");
	        this.checker.clear();
			
			//Run new stuff
			this.test = new Base(this);
			this.t = new Thread(test);	        
	        
			//Manage buttons
			stopButton.setEnabled(false);
	        resumeButton.setEnabled(false);
	        clearButton.setEnabled(false);
	        startButton.setEnabled(true);
			scenarioChoice.setEnabled(true);
			errorFactor.setEnabled(true);
			numberOfSimulations.setEditable(true);
			if (scenarioChoice.getSelectedIndex()==2){
				landmarkChoice.setEnabled(true);
				numberOfLandmarks.setEnabled(true);
				numberOfLandmarks.setEditable(true);
				this.openFile.setEnabled(true);
				this.fileName.setEnabled(true);
			} else {
				landmarkChoice.setEnabled(false);
				numberOfLandmarks.setEnabled(false);
				this.openFile.setEnabled(false);
				this.fileName.setEnabled(false);
			}
			saveGraph.setEnabled(false);
			saveInfo.setEnabled(false);
			if (landmarkChoice.getSelectedIndex()==2) this.getPanelNodes = true;
		} else if (e.getSource() == startButton) {
			try{
				if (this.scenario == -1) this.scenario = this.scenarioChoice.getSelectedIndex() + 1;
				this.test.setCommunication(false);	//By default, do not use communication
				//Generate Landmarks & get network information
				this.userFileName=null;		//Init user file name
				switch (this.scenario){
					case 1:
						this.test.manageLandmarks(Base.READFROMFILE, "default_scenario_1", 0, null);
						break;
					case 2:
						this.test.manageLandmarks(Base.READFROMFILE, "default_scenario_2", 0, null);
						break;
					case 3:
						switch (this.landmarkChoice.getSelectedIndex()) {
						case Base.RANDOM:
							//call function with appropriate set of variables
							this.test.manageLandmarks(Base.RANDOM, "", Integer.parseInt(this.numberOfLandmarks.getText()), null);
							break;
						case Base.READFROMFILE:
							//call function with appropriate set of variables
							this.userFileName = this.fileName.getText();
							this.test.manageLandmarks(Base.READFROMFILE, this.userFileName, 0, null);
							break;
						case Base.READFROMPANEL:
							//call function with appropriate set of variables
							this.test.manageLandmarks(Base.READFROMPANEL, "", 0, this.userLandmarks);
							break;
						case Base.REALNETWORK:
							// do not generate landmarks, use the  nodes received by radio communication 
							this.test.setCommunication(true);
							break;
						default:
							break;
						}
						break;
				}

				//Manage buttons
				stopButton.setEnabled(true);
				startButton.setEnabled(false);
				resumeButton.setEnabled(false);
		        clearButton.setEnabled(false);
				scenarioChoice.setEnabled(false);
				landmarkChoice.setEnabled(false);
				errorFactor.setEnabled(false);
				numberOfLandmarks.setEnabled(false);
				openFile.setEnabled(false);
				saveGraph.setEnabled(false);
				saveInfo.setEnabled(false);
				numberOfSimulations.setEditable(false);
				this.getPanelNodes = false;
				
				//Init infos
				this.userId = 1;
				test.setAntiSlam(this.isAntiSlamed());
				this.scenario = this.scenarioChoice.getSelectedIndex()+1;
				test.setError(this.isErrorEnabled());
				test.setErrorFactor(Integer.parseInt(errorFactor.getText()));
				test.setNumberOfSimulations(Integer.parseInt(numberOfSimulations.getText()));

				this.checker.initChecker(this.scenario,this.landmarkChoice.getSelectedIndex(),this.userFileName,Integer.parseInt(errorFactor.getText()),this.isCompleteSimulation(),this.isComparingAntiSlam(),this.isErrorEnabled(),this.isEkfEnabled(),this.isAntiSlamed(),this.ASChoice.getSelectedIndex()+1);
				this.experimentation.setShift(this.scenario);
				this.test.setScenario(this.scenario);
				this.test.setCompleteSimulation(isCompleteSimulation());
				this.test.setComparingAntiSlam(this.isComparingAntiSlam());
				this.test.setAntiSlamType(this.ASChoice.getSelectedIndex()+1);
				

				String message = "Be careful with FinalDestination!\n"; 
				JOptionPane.showMessageDialog(new JFrame(), message);  
				//Run experimentation
				this.t.start();
				experimentation.repaint();
			
			} catch (FileNotFoundException e1) {
				// Modal dialog with OK button 
				String message = "File to open not found.\nPlease check if it is in \"networks\" folder."; 
				JOptionPane.showMessageDialog(new JFrame(), message); 
			}
			
		} else if (e.getSource() == stopButton) {
			test.stopRoutine();
	        resumeButton.setEnabled(true);
	        clearButton.setEnabled(true);
	        stopButton.setEnabled(false);
	        startButton.setEnabled(false);
	        saveGraph.setEnabled(true);
	        saveInfo.setEnabled(true);
	        this.addStopInfo(false,false);		//Display Metrics
			experimentation.repaint();
		} else if (e.getSource() == resumeButton) {
			test.resumeRoutine();
			resumeButton.setEnabled(false);
	        clearButton.setEnabled(false);
	        startButton.setEnabled(false);
	        stopButton.setEnabled(true);
	        saveGraph.setEnabled(false);
	        saveInfo.setEnabled(false);
	        experimentation.repaint();
		} else if (e.getSource() == viewLandmarks) {
			experimentation.repaint();
		} else if (e.getSource() == viewOdometry) {
			experimentation.repaint();
		} else if (e.getSource() == viewSimulation) {
			if (isCompleteSimulation()) {
				this.scenarioChoice.setSelectedIndex(2);
				
				viewAnti.setVisible(true);
		        viewReal.setVisible(true);
				viewError.setVisible(true);
				viewFilter.setVisible(true);
				ASChoicePanel.setVisible(true);
				enableAntiSlam.setVisible(false);
				enableError.setVisible(false);
				enableEkf.setVisible(false);
				viewTrajectory.setSelected(false);
				enableAntiSlam.setSelected(false);
				enableError.setSelected(false);
				enableEkf.setSelected(false);
				viewTrajectory.setVisible(false);
				this.viewCompareAntiSlam.setSelected(false);
				this.viewCompareAntiSlam.setVisible(false);
			} else {
				viewAnti.setVisible(false);
		        viewReal.setVisible(false);
				viewError.setVisible(false);
				viewFilter.setVisible(false);
				enableAntiSlam.setVisible(true);
				enableError.setVisible(true);
				enableEkf.setVisible(true);
				viewTrajectory.setVisible(true);
				ASChoicePanel.setVisible(false);
				this.viewCompareAntiSlam.setVisible(true);
			}

		} else if (e.getSource() == viewCompareAntiSlam) {
			if (this.isComparingAntiSlam()){
				viewASTranslation.setVisible(true);
				viewASRotation.setVisible(true);
				viewASHybrid.setVisible(true);
				enableAntiSlam.setVisible(false);
				enableError.setVisible(false);
				enableEkf.setVisible(false);
				if (this.scenario!=1) ASChoicePanel.setVisible(false);
				enableAntiSlam.setSelected(false);
				enableError.setSelected(false);
				enableEkf.setSelected(false);
				viewTrajectory.setSelected(true);
				this.viewSimulation.setSelected(false);
				this.viewSimulation.setVisible(false);
			} else {
				viewASTranslation.setVisible(false);
				viewASRotation.setVisible(false);
				viewASHybrid.setVisible(false);
				enableAntiSlam.setVisible(true);
				enableError.setVisible(true);
				enableEkf.setVisible(true);
				this.viewSimulation.setVisible(true);
			}
		} else if (e.getSource() == viewTrajectory) {
			experimentation.repaint();
		} else if (e.getSource() == enableAntiSlam) {
			test.setAntiSlam(this.isAntiSlamed());
			if (this.isAntiSlamed()){
				if (this.scenario!=1) ASChoicePanel.setVisible(true);
			} else {
				ASChoicePanel.setVisible(false);
			}
		} else if (e.getSource() == enableError) {
			test.setError(this.isErrorEnabled());
		} else if (e.getSource() == enableEkf) {
			if (this.isEkfEnabled()) enableError.setSelected(true);
			test.setEkf(this.isEkfEnabled());
		} else if (e.getSource() == this.scenarioChoice) {
			this.scenario = this.scenarioChoice.getSelectedIndex()+1;
			if (this.scenario==1){
				viewVoronoi.setVisible(false);
				viewRange.setVisible(false);
				viewRange.setSelected(false);
			} else {
				viewVoronoi.setVisible(true);
				viewRange.setVisible(true);
				viewRange.setSelected(true);
			}
			if (this.scenario == 3){
				this.landmarkChoice.setEnabled(true);
				this.numberOfLandmarks.setEnabled(true);
				this.numberOfLandmarks.setEditable(true);
			} else {
				this.landmarkChoice.setEnabled(false);
				this.openFile.setEnabled(false);
				this.saveFile.setEnabled(false);
				this.numberOfLandmarks.setEnabled(false);
				this.fileName.setEnabled(false);
			}
		} else if (e.getSource() == this.viewVoronoi) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewAnti) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewReal) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewError) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewFilter) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewRange) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewASTranslation) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewASRotation) {
			experimentation.repaint();
		} else if (e.getSource() == this.viewASHybrid) {
			experimentation.repaint();
		}  else if (e.getSource() == landmarkChoice) {
			this.saved = false;
			switch (this.landmarkChoice.getSelectedIndex()) {
				case Base.RANDOM:
					saveFile.setEnabled(false);
					openFile.setEnabled(false);
					fileName.setEnabled(false);
					numberOfLandmarks.setEditable(true);
					this.getPanelNodes = false;
					break;
				case Base.READFROMFILE:
					openFile.setEnabled(true);
					saveFile.setEnabled(false);
					fileName.setEnabled(true);
					numberOfLandmarks.setEditable(false);
					this.getPanelNodes = false;
					break;
				case Base.READFROMPANEL:
					saveFile.setEnabled(true);
					openFile.setEnabled(false);
					fileName.setEnabled(true);
					numberOfLandmarks.setEditable(false);
					this.getPanelNodes = true;
					break;
				default:
					break;
			}
		} else if (e.getSource() == openFile) {
			this.userFileName = this.fileName.getText();
			try {
				this.test.manageLandmarks(Base.READFROMFILE, this.userFileName, 0, null);
				openFile.setEnabled(false);
			} catch (FileNotFoundException e1) {
				// Modal dialog with OK button 
				String message = "File to open not found\nPlease check if it is in \"network\" folder"; 
				JOptionPane.showMessageDialog(new JFrame(), message); 
			}
		} else if (e.getSource() == saveFile) {
			this.userFileName = this.fileName.getText();
			try {
				this.test.manageLandmarks(Base.SAVECONFIG, this.userFileName, 0, this.userLandmarks);
			} catch (FileNotFoundException e1) {
				//will never appear
			}
			this.saved = true;
			this.saveFile.setEnabled(false);
		} else if (e.getSource() == saveGraph) {
			this.experimentation.saveGraphs();
		} else if (e.getSource() == saveInfo) {
			if ((this.landmarkChoice.getSelectedIndex()==Base.READFROMPANEL)&&(!this.saved)){
				String message = "You have to save the current network before saving metrics."; 
				JOptionPane.showMessageDialog(new JFrame(), message);
			} else {
				this.checker.setFileName(this.fileName.getText());
				this.checker.saveInfo();
			}
		}
	}

	/** See following "See Also".
	 * @see java.awt.event.MouseListener#mouseClicked(java.awt.event.MouseEvent)
	 */
	public void mouseClicked(MouseEvent arg0) {}
	/** See following "See Also".
	 * @see java.awt.event.MouseListener#mouseEntered(java.awt.event.MouseEvent)
	 */
	public void mouseEntered(MouseEvent arg0) {}
	/** See following "See Also".
	 * @see java.awt.event.MouseListener#mouseExited(java.awt.event.MouseEvent)
	 */
	public void mouseExited(MouseEvent arg0) {}
	/** See following "See Also".
	 * @see java.awt.event.MouseListener#mouseReleased(java.awt.event.MouseEvent)
	 */
	public void mouseReleased(MouseEvent arg0) {}
	/** See following "See Also".
	 * @see java.awt.event.MouseListener#mousePressed(java.awt.event.MouseEvent)
	 */
	public void mousePressed(MouseEvent arg0) {
		if ((arg0.getSource() != experimentation)||(!this.getPanelNodes)) return;
		if (this.landmarkChoice.getSelectedIndex() == 2) {
			int[] f = experimentation.getShift();
			this.userLandmarks.add(new double[] {this.userId, (arg0.getX()-f[1])/f[0], -(arg0.getY()-f[2])/f[0]});
			this.userId++;
			this.updateDrawing(null,this.userLandmarks, null, null, null, null);
		}
	}
	
	/**
	 * Tests if viewLandmarks box is checked, if checked landmarks have to be displayed.
	 * @return true if viewLandmarks box checked, false otherwise.
	 */
	public boolean isLandmarks() {
		return viewLandmarks.isSelected();
	}
	/**
	 * Tests if viewOdometry box is checked, if checked current robot position has to be displayed.
	 * @return true if viewOdometry box checked, false otherwise.
	 */
	public boolean isOdometry() {
		return viewOdometry.isSelected();
	}
	/**
	 * Tests if viewTrajectory box is checked, if checked initial robot trajectory has to be displayed.
	 * @return true if viewTrajectory box checked, false otherwise.
	 */
	public boolean isTrajectory() {
		return viewTrajectory.isSelected();
	}
	/**
	 * Tests if enableAntiSlam box is checked, if checked Anti-SLAM has to be enabled during current simulation.
	 * @return true if enableAntiSlam box checked, false otherwise.
	 */
	public boolean isAntiSlamed() {
		return enableAntiSlam.isSelected();
	}
	/**
	 * Tests if enableError box is checked, if checked error has to be enabled during current simulation.
	 * @return true if enableError box checked, false otherwise.
	 */
	public boolean isErrorEnabled() {
		return enableError.isSelected();
	}
	/**
	 * Tests if enableEkf box is checked, if checked EKF has to be enabled during current simulation.
	 * @return true if enableEkf box checked, false otherwise.
	 */
	public boolean isEkfEnabled() {
		return enableEkf.isSelected();
	}
	/**
	 * Tests if viewSimulation box is checked, if checked current simulation will be complete 4-stage simulation.
	 * @return true if viewSimulation box checked, false otherwise.
	 */
	public boolean isCompleteSimulation() {
		return viewSimulation.isSelected();
	}
	/**
	 * Tests if viewCompareAntiSlam box is checked, if checked current simulation will be Anti-SLAM 
	 * comparative simulation.
	 * @return true if viewCompareAntiSlam box checked, false otherwise.
	 */
	public boolean isComparingAntiSlam() {
		return viewCompareAntiSlam.isSelected();
	}
	/**
	 * Tests if viewVoronoi box is checked, if checked Voronoi edges have to be displayed.
	 * @return true if viewVoronoi box checked, false otherwise.
	 */
	public boolean viewVoronoiEnabled() {
		return viewVoronoi.isSelected();
	}

	/**
	 * Tests if viewRange box is checked, if checked landmarks' communication range have to be displayed.
	 * @return true if viewRange box checked, false otherwise.
	 */
	public boolean viewRangeEnabled() {
		return this.viewRange.isSelected();
	}

	/**
	 * Tests if viewAnti box is checked, if checked real landmark (as generated) have to be displayed.
	 * @return true if viewAnti box checked, false otherwise.
	 */
	public boolean viewAntiEnabled() {
		return this.viewAnti.isSelected();
	}

	/**
	 * Tests if viewReal box is checked, if checked initial robot trajectory has to be displayed during 
	 * complete 4-stage simulation.
	 * @return true if viewReal box checked, false otherwise.
	 */
	public boolean viewRealEnabled() {
		return this.viewReal.isSelected();
	}

	/**
	 * Tests if viewError box is checked, if checked error robot trajectory has to be displayed during 
	 * complete 4-stage simulation.
	 * @return true if viewError box checked, false otherwise.
	 */
	public boolean viewErrorEnabled() {
		return this.viewError.isSelected();
	}

	/**
	 * Tests if viewASTranslation box is checked, if checked robot trajectory has to be displayed, in a translation
	 * Anti-SLAM strategy, during Anti-SLAM comparative simulation.
	 * @return true if viewASTranslation box checked, false otherwise.
	 */
	public boolean viewASTranslationEnabled() {
		return this.viewASTranslation.isSelected();
	}

	/**
	 * Tests if viewASRotation box is checked, if checked robot trajectory has to be displayed, in a rotation
	 * Anti-SLAM strategy, during Anti-SLAM comparative simulation.
	 * @return true if viewASRotation box checked, false otherwise.
	 */
	public boolean viewASRotationEnabled() {
		return this.viewASRotation.isSelected();
	}

	/**
	 * Tests if viewASHybrid box is checked, if checked robot trajectory has to be displayed, in a hybrid
	 * Anti-SLAM strategy, during Anti-SLAM comparative simulation.
	 * @return true if viewASHybrid box checked, false otherwise.
	 */
	public boolean viewASHybridEnabled() {
		return this.viewASHybrid.isSelected();
	}

	/**
	 * Tests if viewTrajectory box is checked, if checked initial robot trajectory has to be displayed.
	 * @return true if viewTrajectory box checked, false otherwise.
	 */
	public boolean viewFilterEnabled() {
		return this.viewFilter.isSelected();
	}

	/**
	 * Add specified string in message log panel.
	 * @param text the text to display.
	 */
	public void addMsgLog(String text) {
		this.messageLog.append(text);
	}
	
	/**
	 * Display all metric informations in appropriate panel, save them in a file then clear them if specified.
	 * @param saveInfo true if info have to been saved, false they just have to been displayed.
	 * @param end true if the whole simulation is finished and the interface has to be reinitialized, 
	 * false otherwise.
	 */
	public void addStopInfo(boolean saveInfo, boolean end){
		if (end) {	//End simulation (robot detected or reached its final destination)
	        resumeButton.setEnabled(false);
	        clearButton.setEnabled(true);
	        stopButton.setEnabled(false);
	        startButton.setEnabled(false);
	        saveGraph.setEnabled(true);
		} else {	//Simulation not finished, do not reinitialized the interface
			this.M1Display.append(this.checker.verifyData(DataChecker.displayM1));
			this.M2Display.append(this.checker.verifyData(DataChecker.displayM2));
			this.M3Display.append(this.checker.verifyData(DataChecker.displayM3));
			this.M4Display.append(this.checker.verifyData(DataChecker.displayM4));
			
			if (saveInfo) {
				this.checker.saveInfo();
				if ((this.isCompleteSimulation())||(this.isComparingAntiSlam())||(Integer.parseInt(numberOfSimulations.getText())>1)){
					if (saveInfo) this.checker.clear();
					this.debugLog.setText("");
				}
			} else this.saveInfo.setEnabled(true);
		}
	}
	
	public void reInitChecker (){
		this.checker.clear();
		this.debugLog.setText("");
	}
		
	/**
	 * Add specified data in a correct format according to the specified type in the debug log panel.
	 * @param type the type of data to display.
	 * @param data the data to display.
	 */
	public void addDebugLog(int type, double... data) {
		this.debugLog.append(this.checker.verifyData(type, data));
	}
		
		/**
	 * Set all data to draw in the interface, stop the simulation if the robot is in an acting range of at 
	 * least one node.
	 * @param position all previous and current positions of the robot in an initial trajectory.
	 * @param landmarks all current known landmarks.
	 * @param errorPosition all previous and current positions of the robot in an error trajectory.
	 * @param estimatePosition all previous and current positions of the robot in an EKF trajectory.
	 * @param antislamedPosition all previous and current positions of the robot in an Anti-SLAM trajectory.
	 * @param polygons all current Voronoi polygons.
	 */
	public void updateDrawing(Vector<double[]> position, Vector<double[]> landmarks, Vector<double[]> errorPosition, Vector<double[]> estimatePosition, Vector<double[]> antislamedPosition,Vector<Point2D[]> polygons) {
		experimentation.updateData(position, null, landmarks, errorPosition, estimatePosition, antislamedPosition, polygons);
		experimentation.repaint();
		
		if (this.checker.isInActingRange()){
			this.test.setDetected(true);
			if (!this.isComparingAntiSlam()){
				this.test.stopRoutine();
			}
		}
	}

	/**
	 * Set the real landmarks to draw in the interface if necessary.
	 * @param realLandmarks the real landmarks as generated or as received before processing them.
	 */
	public void setRealLandmarks(Vector<double[]> realLandmarks) {
		this.checker.setRealLandmarks(realLandmarks);
		this.experimentation.updateData(null, realLandmarks, null, null, null, null, null);
	}
	
	/***/
	public void setSimulationNumber(int number){
		this.checker.setSimulationNumber(number);
	}
	
	/**
	 * Main function, used to launch the applet.
	 * @param args execution parameters, not used.
	 */
	public static void main (String[] args) {
		WSNexperimentation applet = new WSNexperimentation();
        applet.init();
        JFrame dWindow = new JFrame();
        dWindow.setExtendedState(dWindow.getExtendedState()|JFrame.MAXIMIZED_BOTH);
        dWindow.setTitle("WSN Experimentation");
        dWindow.setLayout(new BorderLayout());
        dWindow.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
        dWindow.add(applet, "Center");
        dWindow.add(new Label(" Lionel Croix | Anis Darwich | Simulation and experimentation on Robot in WSN | WnSN Lab | SJTU "), "South");
        dWindow.setVisible(true);
    }
}


/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 *
 */
class DrawPanel extends JPanel implements Printable {

	/**Universal version IDentifier for Serializable class.*/
	private static final long serialVersionUID = 1L;
	
	/**Object corresponding to the Base station implementation.*/
	private WSNexperimentation xPeriment;

	/**Current scenario.*/
	private int scenario;
	/**Drawing factor to a correct display.*/
	private int factor;
	/**Drawing shift on X coordinate.*/
	private int xShift;
	/**Drawing shift on Y coordinate.*/
	private int yShift;

	/**Drawing Panel Object.*/
	private Graphics2D g;

	/**Set of all previous and current robot position.*/
	private Vector<double[]> position = null;
	/**Set of all real landmarks (as generated).*/
	private Vector<double[]> realLandmarks = null;
	/**Set of all known landmarks (after being processed).*/
	private Vector<double[]> landmarks = null;
	/**Set of all previous and current robot position in an EKF trajectory.*/
	private Vector<double[]> estimate = null;
	/**Set of all previous and current robot position in an error trajectory.*/
	private Vector<double[]> error = null;
	/**Set of all previous and current robot position in an Anti-SLAM trajectory.*/
	private Vector<double[]> antislamed = null;
	/**Set of all current Voronoi polygons.*/
	private Vector<Point2D[]> polygons;

	/**
	 * Constructs the DrawPanel object and define drawing shift.
	 * @param wsNexperimentation the object representing the Base Station implementation.
	 * @param scenario the current value of simulation's scenario.
	 */
	public DrawPanel(WSNexperimentation wsNexperimentation,int scenario) {
		this.xPeriment = wsNexperimentation;
		this.setShift(scenario);
	}
	
	/**
	 * Clear all DrawPanel attributes and refresh the panel.
	 */
	public void clear() {
		this.position = null;
		this.landmarks = null;
		this.estimate = null;
		this.error = null;
		this.polygons = null;
		this.antislamed = null;
		this.realLandmarks = null;
		this.repaint();
	}

	/**
	 * Set all data used for drawing.
	 * @param position all previous and current positions of the robot in an initial trajectory.
	 * @param realLandmarks all real landmarks (as generated).
	 * @param landmarks all current known landmarks.
	 * @param errorPosition all previous and current positions of the robot in an error trajectory.
	 * @param estimatePosition all previous and current positions of the robot in an EKF trajectory.
	 * @param antislamedPosition all previous and current positions of the robot in an Anti-SLAM trajectory.
	 * @param polygons all current Voronoi polygons.
	 */
	public void updateData(Vector<double[]> position, Vector<double[]> realLandmarks,  Vector<double[]> landmarks, Vector<double[]> errorPosition, Vector<double[]> estimatePosition, Vector<double[]> antislamedPosition, Vector<Point2D[]> polygons) {
		this.position = position;
		if (realLandmarks!=null) this.realLandmarks = realLandmarks;
		this.landmarks = landmarks;
		this.estimate = estimatePosition;
		this.error = errorPosition;
		this.antislamed = antislamedPosition;
		this.polygons = polygons;
	}
	
	/**
	 * Set the drawing shifts according to the specified scenario.
	 * @param scenario the current scenario.
	 */
	public void setShift(int scenario){
		this.scenario = scenario;
		if (this.scenario==1){
			this.factor = 10;
			this.xShift = 20*this.factor;
			this.yShift = 30*this.factor;
		} else {
			this.factor = 5;
			this.xShift = 32*this.factor;
			this.yShift = 57*this.factor;
		}
	}
	
	/**
	 * Get informations of current drawing shift.
	 * @return the int array containing shift parameters [factor, X coordinate shift, Y coordinate shift].
	 */
	public int[] getShift() {
		return new int[] {this.factor, this.xShift, this.yShift};
	}
	
	/**
	 * Draw all known informations according to checkboxes.
	 * @see javax.swing.JComponent#paintComponent(java.awt.Graphics)
	 * 
	 */
	public void paintComponent(Graphics g) {
		this.g = (Graphics2D) g;
		super.paintComponent(g);
		
		int i;
		double[] tmp, tmp2;

		if (xPeriment.viewVoronoiEnabled()){
			if (this.polygons != null) {
				Point2D[] polygon = null;;
				int j=0;
				for (i = 0;i<polygons.size();i++){
					polygon = polygons.get(i);
					Point2D[] points = new Point2D[polygon.length];
					for (j = 0; j < polygon.length; j++) {
						points[j] = new Point2D.Double(polygon[j].getX()*factor + xShift, -polygon[j].getY()*factor + yShift);
						if ((j>0) && (j <= polygon.length-1)) {
							this.g.setColor(Color.white);
							this.g.draw(new Line2D.Double(points[j-1],points[j]));
						}
						if (j == polygon.length-1){
							this.g.setColor(Color.white);
							this.g.draw(new Line2D.Double(points[0],points[j]));
						}
					}
				}
			}
		}

		if (xPeriment.isLandmarks()) {
			if ((this.xPeriment.isAntiSlamed())||(this.xPeriment.isComparingAntiSlam())){
				if (this.scenario==1){
					this.landmarks = new Vector<double[]>(5,1);
					this.landmarks.add(new double[] {1, 0, 20});
					this.landmarks.add(new double[] {2, 20, 20});
					this.landmarks.add(new double[] {3, 20, 0});
					this.landmarks.add(new double[] {4, 0, 0});
					this.landmarks.add(new double[] {5, 10, 10});	
				} else {
					if ((this.realLandmarks!=null)&&(this.landmarks!=null)){
						for(i = 0; i < realLandmarks.size(); i++) {
							this.g.setColor(Color.gray);
							tmp = realLandmarks.get(i);
							this.g.drawString("["+(int)tmp[0]+"] " + (int)tmp[1] + "," + (int)tmp[2], (int)tmp[1]*factor + xShift, -(int)tmp[2]*factor + yShift);
							this.g.fillRect((int)tmp[1]*factor + xShift, -(int)tmp[2]*factor + yShift, 4, 4);
							if (this.xPeriment.viewRangeEnabled()){
								this.g.setColor(Color.gray);
								this.g.setStroke( new BasicStroke(1,BasicStroke.CAP_BUTT,BasicStroke.JOIN_MITER,10.0f,new float[]{5,5},0));
								this.g.drawOval((int)tmp[1]*factor + xShift-DataChecker.communicationRange*factor, -(int)tmp[2]*factor + yShift-DataChecker.communicationRange*factor, 2*DataChecker.communicationRange*factor, 2*DataChecker.communicationRange*factor);
								this.g.drawOval((int)tmp[1]*factor + xShift-DataChecker.actingRange*factor, -(int)tmp[2]*factor + yShift-DataChecker.actingRange*factor, 2*DataChecker.actingRange*factor, 2*DataChecker.actingRange*factor);
								//this.g.drawOval((int)tmp[1]*factor + xShift-DataChecker.sensingRange*factor, -(int)tmp[2]*factor + yShift-DataChecker.sensingRange*factor, 2*DataChecker.sensingRange*factor, 2*DataChecker.sensingRange*factor);
							}
						}
					}
				}
			}
			if (landmarks != null) {
				for(i = 0; i < landmarks.size(); i++) {
					this.g.setColor(Color.black);
					tmp = landmarks.get(i);
					this.g.drawString("["+(int)tmp[0]+"] " + (int)tmp[1] + "," + (int)tmp[2], (int)tmp[1]*factor + xShift, -(int)tmp[2]*factor + yShift);
					this.g.fillRect((int)tmp[1]*factor + xShift, -(int)tmp[2]*factor + yShift, 4, 4);
					if (this.xPeriment.viewRangeEnabled()){
						this.g.setColor(Color.darkGray);
						this.g.setStroke( new BasicStroke(1,BasicStroke.CAP_BUTT,BasicStroke.JOIN_MITER,10.0f,new float[]{5,5},0));
						this.g.drawOval((int)tmp[1]*factor + xShift-DataChecker.communicationRange*factor, -(int)tmp[2]*factor + yShift-DataChecker.communicationRange*factor, 2*DataChecker.communicationRange*factor, 2*DataChecker.communicationRange*factor);
					}
				}
			}
		}

		if ((xPeriment.isTrajectory())||(xPeriment.viewRealEnabled())) {
			if (position != null) {
				for(i = 0; i < position.size()-1; i++) {
					tmp = position.get(i);
					tmp2 = position.get(i+1);
					this.g.setColor(Color.blue);
					this.g.setStroke(new BasicStroke( 1.5f ));
					this.g.draw(new Line2D.Double(tmp[0]*factor + xShift, -tmp[1]*factor + yShift, tmp2[0]*factor + xShift, -tmp2[1]*factor + yShift));
				}
			}
		} 

		if ((xPeriment.viewErrorEnabled())||(xPeriment.viewASTranslationEnabled())){
			if (error != null) {
				for(i = 0; i < error.size()-1; i++) {
					tmp = error.get(i);
					tmp2 = error.get(i+1);
					this.g.setColor(Color.green);
					this.g.draw(new Line2D.Double(tmp[0]*factor + xShift, -tmp[1]*factor + yShift, tmp2[0]*factor + xShift, -tmp2[1]*factor + yShift));
				}
			}
		}

		if ((xPeriment.viewFilterEnabled())||(xPeriment.viewASRotationEnabled())){
			if (estimate != null) {
				for(i = 0; i < estimate.size()-1; i++) {
					tmp = estimate.get(i);
					tmp2 = estimate.get(i+1);
					this.g.setColor(Color.red);
					this.g.draw(new Line2D.Double(tmp[0]*factor + xShift, -tmp[1]*factor + yShift, tmp2[0]*factor + xShift, -tmp2[1]*factor + yShift));
				}
			}
		} 

		if ((xPeriment.viewAntiEnabled())||(xPeriment.viewASHybridEnabled())){
			if (antislamed != null) {
				for(i = 0; i < antislamed.size()-1; i++) {
					tmp = antislamed.get(i);
					tmp2 = antislamed.get(i+1);
					this.g.setColor(Color.orange);
					this.g.draw(new Line2D.Double(tmp[0]*factor + xShift, -tmp[1]*factor + yShift, tmp2[0]*factor + xShift, -tmp2[1]*factor + yShift));
				}
			}
		}


		if (xPeriment.isOdometry()) {
			if ((position != null) && (position.size() > 1)) {
				tmp = position.get(position.size()-1);
				this.g.setColor(Color.magenta);
				this.g.draw(new RoundRectangle2D.Double(tmp[0]*factor + xShift-5, -tmp[1]*factor + yShift-5, 10, 10, 200, 200));
			}
			if ((estimate != null) && (estimate.size() > 1)) {
				tmp = estimate.get(estimate.size()-1);
				this.g.setColor(Color.magenta);
				this.g.draw(new RoundRectangle2D.Double(tmp[0]*factor + xShift-5, -tmp[1]*factor + yShift-5, 10, 10, 200, 200));
			}
			if ((error != null) && (error.size() > 1)) {
				tmp = error.get(error.size()-1);
				this.g.setColor(Color.magenta);
				this.g.draw(new RoundRectangle2D.Double(tmp[0]*factor + xShift-5, -tmp[1]*factor + yShift-5, 10, 10, 200, 200));
			}
			if ((antislamed != null) && (antislamed.size() > 1)) {
				tmp = antislamed.get(antislamed.size()-1);
				this.g.setColor(Color.magenta);
				this.g.draw(new RoundRectangle2D.Double(tmp[0]*factor + xShift-5, -tmp[1]*factor + yShift-5, 10, 10, 200, 200));
			}
		}

	}
	
	/**
	 * Open an dialog box to print the current drawing.
	 */
	public void saveGraphs() {
		PrinterJob printJob = PrinterJob.getPrinterJob();
        printJob.setPrintable(this);
        if (printJob.printDialog()) {
            try {
                printJob.print();  
            } catch (Exception PrintException) {
                PrintException.printStackTrace();
            }
        }
	}

	/** See following "See Also".
	 * @see java.awt.print.Printable#print(java.awt.Graphics, java.awt.print.PageFormat, int)
	 */
	public int print(Graphics graphics, PageFormat pageFormat, int pageIndex) throws PrinterException {
		if (pageIndex >= 1) {
            return Printable.NO_SUCH_PAGE;
		}
		Graphics2D g2d = (Graphics2D) graphics;
        g2d.translate(pageFormat.getImageableX(), pageFormat.getImageableY());
        this.setBackground(Color.white);
        disableDoubleBuffering(this);
        this.paint(g2d);
        enableDoubleBuffering(this);
        this.setBackground(Color.lightGray);
        return Printable.PAGE_EXISTS;
	}
	
	/**
	 * Disable the double buffering display of the specified Panel.
	 * @param c the panel of which the double buffering display will be disabled.
	 */
	public static void disableDoubleBuffering(Component c) {
	    RepaintManager currentManager = RepaintManager.currentManager(c);
	    currentManager.setDoubleBufferingEnabled(false);
	  }
	
	/**
	 * Enable the double buffering display of the specified Panel.
	 * @param c the panel of which the double buffering display will be enabled.
	 */
	public static void enableDoubleBuffering(Component c) {
		RepaintManager currentManager = RepaintManager.currentManager(c);
	    currentManager.setDoubleBufferingEnabled(true);
	}
}